/*
 * File:         khepera_bus.c
 * Date:         September 4th, 2006
 * Description:  A controller for the Khepera robot intended to be
 *               cross-compiled in order to run on the real robot.
 * Author:       Simon Blanchoud
 * Modifications:
 *
 * Copyright (c) 2008 Cyberbotics - www.cyberbotics.com
 */

#include <webots/DifferentialWheels.hpp>
#include <webots/Receiver.hpp>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <iostream>
#include <fstream>

#define FORWARD_SPEED 8
#define TURN_SPEED 5
#define SENSOR_THRESHOLD 150
#define NB_SENSOR 8
#define TIME_STEP 64


/*
 * The RS232 communication system is useful to send commands to a real
 * Khepera robot using the Khepera protocol, this way, it is possible for
 * example to retrive information from custom extension turret using the
 * "T" protocol command. The example here shows how the Khepera robot
 * answers to the "B" command, asking for software versions.
 * This system will be ignored in simulation and with the cross-compiled
 * version of this controller.
 */


int main(int argc, char ** argv)
{
    const double * data;
    webots::DifferentialWheels khep;
    webots::Receiver* rec=khep.getReceiver("rs232_in");
    rec->enable(10);
    rec->setChannel(argv[1][0]-48);
    for (;;) {                  /* The robot never dies! */
usleep(1000);
        if(rec->getQueueLength()>0)
        {
            data=(const double*)rec->getData();
            rec->nextPacket();
            khep.setSpeed(data[0],data[1]); /* Set the motor speeds */
        }
        khep.step(TIME_STEP);         /* run one step */
    }
    return 0;
}
